#!/usr/bin/python2.7
# coding=UTF-8
import rospy
import time
import math
from geometry_msgs.msg import Point, PoseStamped, PointStamped
from mavros_msgs.msg import State
import numpy as np


class PID_controller:
    def __init__(self):
        # mavros state
        self.mavros_state = State()
        # control param
        self.K_P_pv = [1.5, 2.0, 2.0, 2.0, 2.0, 3.5]  # order (x,y,z,xv,yv,zv)
        self.K_D_pv = [0, 0, 0, 0.02, 0.02, 0.02]  # order (x,y,z,xv,yv,zv)
        self.K_I_pv = [0.00, 0.0, 0.0, 0.2, 0.2, 0.5]  # order (x,y,z,xv,yv,zv)
        self.limit = [3, 3, 3]  # position control limit  -- in world frame
        self.limit_I = [2, 2, 2, 1, 1, 1]  # order (x,y,z,xv,yv,zv)
        self.error_length_I = 15
        self.deathband = [0.005, 0.005, 0.005]  # control deathband
        self.tol = 0.4  # position control tolerence
        # self.hover_throttle = 0.5644  # hover_throttle: throttle required for uav hovering
        self.hover_throttle = 0.53  # hover_throttle: throttle required for uav hovering
        self.gravity = 9.794

        # pva control param
        # self.K_P_pva = [3.0, 3.0, 2.0, 2.0, 2.0, 3.5]  # order p,v
        self.K_P_pva = [1.5, 2.0, 2.0, 2.0, 2.0, 3.5]  # order p,v


        self.K_D_pva = [0, 0, 0, 0.02, 0.02, 0.02]  # order p,v
        self.K_I_pva = [0.0, 0.0, 0.0, 0.02, 0.02, 0.5]  # order p,v

        # uav state
        self.time = rospy.get_time()
        self.position = [0, 0, 0]
        self.velocity = [0, 0, 0]

        # error
        self.position_error_last = [0, 0, 0]
        self.velocity_error_last = [0, 0, 0]
        self.forward_error = 0

        # integral error
        self.i_start_flag = False
        self.i_in_position = [0, 0, 0]
        self.i_in_velocity = [0, 0, 0]
        self.error_list_position = [[], [], []]
        self.error_list_velocity = [[], [], []]

        # param in cross circle
        self.target_forward_velocity = 2.0

        # subscribe mavros state
        self.mavros_sub = rospy.Subscriber("/mavros/state", State, self.mavros_state_callback)

    # mavros callback
    def mavros_state_callback(self, msg):
        if msg.mode == "offboard":
            self.i_start_flag = True

    # set point in attitude
    def set_point_in_attitude(self, target_x, target_y, target_z, cur_x, cur_y, cur_z, velocity, time, yaw):
        """
        :param:  target position:(target_x,target_y,target_z)
                 current position:(cur_x,cur_y,cur_z)
                 current velocity: velocity(list 1*3)
                 time: time now
                 yaw: uav yaw (rad)
        :return: row,pith,yaw,thrust
        """
        # ros pub
        velocity_uav_pub = rospy.Publisher('/uav/velocity_in_uav', PointStamped, queue_size=10)
        position_error_pub = rospy.Publisher('/error/position_FLU', PointStamped, queue_size=10)
        position_error_world_pub = rospy.Publisher('/error/position_ENU', PointStamped, queue_size=10)
        velocity_error_pub = rospy.Publisher('/error/velocity', PointStamped, queue_size=10)
        control_uav_pub = rospy.Publisher('/control', PointStamped, queue_size=10)
        velocity_uav = PointStamped()
        position_error_uav = PointStamped()
        position_error_ENU = PointStamped()
        velocity_error = PointStamped()
        control_uav = PointStamped()
        
        # state init
        dt = time - self.time
        self.velocity = velocity

        # calculate velocity in body frame(FLU)
        x_velocity_in_uav, y_velocity_in_uav, z_velocity_in_uav = self.ENU2FLU(velocity[0], velocity[1],
                                                                               velocity[2], yaw)

        # calculate error
        x_error = target_x - cur_x
        y_error = target_y - cur_y
        z_error = target_z - cur_z

        #  calculate error in body frame(FLU)
        x_error_in_uav, y_error_in_uav, z_error_in_uav = self.ENU2FLU(x_error, y_error, z_error, yaw)

        # calculate velocity error in body frame(FLU)
        xv_error_in_uav_new = x_error_in_uav * self.K_P_pv[0] + (x_error_in_uav - self.position_error_last[0]) / dt * \
                              self.K_D_pv[0] + self.i_in_position[0] * self.K_I_pv[0] - x_velocity_in_uav
        yv_error_in_uav_new = y_error_in_uav * self.K_P_pv[1] + (y_error_in_uav - self.position_error_last[1]) / dt * \
                              self.K_D_pv[1] + self.i_in_position[1] * self.K_I_pv[1] - y_velocity_in_uav
        zv_error_new = z_error * self.K_P_pv[2] + (z_error - self.position_error_last[2]) / dt * \
                       self.K_D_pv[2] + self.i_in_position[2] * self.K_I_pv[2] - velocity[2]

        # calculate control out in body frame
        x_out = self.K_P_pv[3] * xv_error_in_uav_new + self.K_D_pv[3] * (
                xv_error_in_uav_new - self.velocity_error_last[0]) / dt + self.K_I_pv[3] * self.i_in_velocity[0]
        y_out = self.K_P_pv[4] * yv_error_in_uav_new + self.K_D_pv[4] * (
                yv_error_in_uav_new - self.velocity_error_last[1]) / dt + self.K_I_pv[4] * self.i_in_velocity[1]

        z_out = self.K_P_pv[5] * zv_error_new + self.K_D_pv[5] * (zv_error_new - self.velocity_error_last[2]) / dt + \
                self.K_I_pv[5] * self.i_in_velocity[2]

        # filter out and add control deathband
        x_out, y_out, z_out = self.filter_control_out(x_out, y_out, z_out)
        z_out = z_out + self.gravity

        # calculate row pith yaw
        roll = math.asin(
            -y_out / np.sqrt(x_out * x_out + y_out * y_out + z_out * z_out))
        pitch = math.atan(x_out / z_out)
        thrust = np.sqrt(
            x_out * x_out + y_out * y_out + z_out * z_out) / self.gravity * self.hover_throttle  # 0.594 - hover thrust
        # print('roll', roll)
        # print('pitch', pitch)
        # print('thrust', thrust)

        # set the history of control error
        self.position_error_last[0] = x_error_in_uav
        self.position_error_last[1] = y_error_in_uav
        self.position_error_last[2] = z_error_in_uav  # position

        self.velocity_error_last[0] = xv_error_in_uav_new
        self.velocity_error_last[1] = yv_error_in_uav_new
        self.velocity_error_last[2] = zv_error_new  # velocity

        # set the history of I error
        if self.i_start_flag:
            self.error_list_position[0].append(x_error_in_uav * dt)
            self.error_list_position[1].append(y_error_in_uav * dt)
            self.error_list_position[2].append(z_error * dt)
            self.error_list_velocity[0].append(xv_error_in_uav_new * dt)
            self.error_list_velocity[1].append(yv_error_in_uav_new * dt)
            self.error_list_velocity[2].append(zv_error_new * dt)
            self.filter_i_out()

        # set the time
        self.time = time

        # ros publish
        velocity_uav.header.stamp = rospy.Time.now()
        velocity_uav.point.x = x_velocity_in_uav
        velocity_uav.point.y = y_velocity_in_uav
        velocity_uav.point.z = z_velocity_in_uav
        velocity_uav_pub.publish(velocity_uav)

        position_error_uav.header.stamp = rospy.Time.now()
        position_error_uav.point.x = x_error_in_uav
        position_error_uav.point.y = y_error_in_uav
        position_error_uav.point.z = z_error
        position_error_pub.publish(position_error_uav)

        position_error_ENU.header.stamp = rospy.Time.now()
        position_error_ENU.point.x = x_error
        position_error_ENU.point.y = y_error
        position_error_ENU.point.z = z_error
        position_error_world_pub.publish(position_error_ENU)

        velocity_error.header.stamp = rospy.Time.now()
        velocity_error.point.x = xv_error_in_uav_new
        velocity_error.point.y = yv_error_in_uav_new
        velocity_error.point.z = zv_error_new
        velocity_error_pub.publish(velocity_error)

        control_uav.header.stamp = rospy.Time.now()
        control_uav.point.x = x_out
        control_uav.point.y = y_out
        control_uav.point.z = z_out
        control_uav_pub.publish(control_uav)
        # print('publish succeed')

        return roll, pitch, thrust

    # set vx and y,z
    def set_vx_in_attitude(self, target_x, target_y, target_z, cur_x, cur_y, cur_z, velocity, time, yaw):
        """
        :param:  target position:(target_x,target_y,target_z)
                 current position:(cur_x,cur_y,cur_z)
                 current velocity: velocity(list 1*3)
                 time: time now
                 yaw: uav yaw (rad)
        :return: row,pith,yaw,thrust
        """
        # ros pub
        velocity_uav_pub = rospy.Publisher('/uav/velocity_in_uav', PointStamped, queue_size=10)
        position_error_pub = rospy.Publisher('/error/position_FLU', PointStamped, queue_size=10)
        position_error_world_pub = rospy.Publisher('/error/position_ENU', PointStamped, queue_size=10)
        velocity_error_pub = rospy.Publisher('/error/velocity', PointStamped, queue_size=10)
        control_uav_pub = rospy.Publisher('/control', PointStamped, queue_size=10)
        velocity_uav = PointStamped()
        position_error_uav = PointStamped()
        position_error_ENU = PointStamped()
        velocity_error = PointStamped()
        control_uav = PointStamped()

        # state init
        dt = time - self.time
        self.velocity = velocity

        # calculate velocity in body frame(FLU)
        x_velocity_in_uav, y_velocity_in_uav, z_velocity_in_uav = self.ENU2FLU(velocity[0], velocity[1],
                                                                               velocity[2], yaw)

        # calculate error
        x_error = target_x - cur_x
        y_error = target_y - cur_y
        z_error = target_z - cur_z

        #  calculate error in body frame(FLU)
        x_error_in_uav, y_error_in_uav, z_error_in_uav = self.ENU2FLU(x_error, y_error, z_error, yaw)

        error = np.sqrt(x_error * x_error + y_error * y_error + z_error * z_error)

        # calculate velocity error in body frame(FLU)
        xv_error_in_uav_new = x_error_in_uav * self.K_P_pv[0] + (x_error_in_uav - self.position_error_last[0]) / dt * \
                              self.K_D_pv[0] + self.i_in_position[0] * self.K_I_pv[0] - x_velocity_in_uav
        yv_error_in_uav_new = y_error_in_uav * self.K_P_pv[1] + (y_error_in_uav - self.position_error_last[1]) / dt * \
                              self.K_D_pv[1] + self.i_in_position[1] * self.K_I_pv[1] - y_velocity_in_uav
        zv_error_new = z_error * self.K_P_pv[2] + (z_error - self.position_error_last[2]) / dt * \
                       self.K_D_pv[2] + self.i_in_position[2] * self.K_I_pv[2] - velocity[2]

        # set the forward velocity
        xv_error_in_uav_new = self.target_forward_velocity - x_velocity_in_uav

        # calculate control out in body frame
        x_out = self.K_P_pv[3] * xv_error_in_uav_new + self.K_D_pv[3] * (
                xv_error_in_uav_new - self.velocity_error_last[0]) / dt + self.K_I_pv[3] * self.i_in_velocity[0]
        y_out = self.K_P_pv[4] * yv_error_in_uav_new + self.K_D_pv[4] * (
                yv_error_in_uav_new - self.velocity_error_last[1]) / dt + self.K_I_pv[4] * self.i_in_velocity[1]

        z_out = self.K_P_pv[5] * zv_error_new + self.K_D_pv[5] * (zv_error_new - self.velocity_error_last[2]) / dt + \
                self.K_I_pv[5] * self.i_in_velocity[2]

        # filter out and add control deathband
        x_out, y_out, z_out = self.filter_control_out(x_out, y_out, z_out)
        z_out = z_out + self.gravity

        # calculate row pith yaw
        roll = math.asin(
            - y_out / np.sqrt(x_out * x_out + y_out * y_out + z_out * z_out))
        pitch = math.atan(x_out / z_out)
        thrust = np.sqrt(
            x_out * x_out + y_out * y_out + z_out * z_out) / self.gravity * self.hover_throttle  # 0.594 - hover thrust
        # print('roll', roll)
        # print('pitch', pitch)
        # print('thrust', thrust)

        # set the history of control error
        self.position_error_last[0] = x_error_in_uav
        self.position_error_last[1] = y_error_in_uav
        self.position_error_last[2] = z_error_in_uav  # position

        self.velocity_error_last[0] = xv_error_in_uav_new
        self.velocity_error_last[1] = yv_error_in_uav_new
        self.velocity_error_last[2] = zv_error_new  # velocity

        # set the history of I error
        if self.i_start_flag:
            self.error_list_position[0].append(x_error_in_uav * dt)
            self.error_list_position[1].append(y_error_in_uav * dt)
            self.error_list_position[2].append(z_error * dt)
            self.error_list_velocity[0].append(xv_error_in_uav_new * dt)
            self.error_list_velocity[1].append(yv_error_in_uav_new * dt)
            self.error_list_velocity[2].append(zv_error_new * dt)
            self.filter_i_out()

        # set the time
        self.time = time

        # ros publish
        velocity_uav.header.stamp = rospy.Time.now()
        velocity_uav.point.x = x_velocity_in_uav
        velocity_uav.point.y = y_velocity_in_uav
        velocity_uav.point.z = z_velocity_in_uav
        velocity_uav_pub.publish(velocity_uav)

        position_error_uav.header.stamp = rospy.Time.now()
        position_error_uav.point.x = x_error_in_uav
        position_error_uav.point.y = y_error_in_uav
        position_error_uav.point.z = z_error
        position_error_pub.publish(position_error_uav)

        position_error_ENU.header.stamp = rospy.Time.now()
        position_error_ENU.point.x = x_error
        position_error_ENU.point.y = y_error
        position_error_ENU.point.z = z_error
        position_error_world_pub.publish(position_error_ENU)

        velocity_error.header.stamp = rospy.Time.now()
        velocity_error.point.x = xv_error_in_uav_new
        velocity_error.point.y = yv_error_in_uav_new
        velocity_error.point.z = zv_error_new
        velocity_error_pub.publish(velocity_error)

        control_uav.header.stamp = rospy.Time.now()
        control_uav.point.x = x_out
        control_uav.point.y = y_out
        control_uav.point.z = z_out
        control_uav_pub.publish(control_uav)

        # print('publish succeed')

        return roll, pitch, thrust

    def set_pva_in_attitude(self, target_p, target_v, target_a, cur_p, cur_velocity, time, yaw):
        """
        :param:  target position:target_p [target_x,target_y,target_z]
                 target velocity: target_v [target_vx,target_vy,target_vz]
                 target accelerate: target_a [target_ax,target_ay,target_az]
                 current position:cur_p [cur_x,cur_y,cur_z]
                 current velocity: velocity(list 1*3)
                 time: time now
                 yaw: uav yaw (rad)
        :return: row,pith,yaw,thrust
        """
        # ros pub
        velocity_uav_pub = rospy.Publisher('/uav/velocity_in_uav', PointStamped, queue_size=10)
        position_error_pub = rospy.Publisher('/error/position_FLU', PointStamped, queue_size=10)
        position_error_world_pub = rospy.Publisher('/error/position_ENU', PointStamped, queue_size=10)
        velocity_error_pub = rospy.Publisher('/error/velocity', PointStamped, queue_size=10)
        control_uav_pub = rospy.Publisher('/control/ENU', PointStamped, queue_size=10)
        velocity_uav = PointStamped()
        position_error_uav = PointStamped()
        velocity_error = PointStamped()
        control_uav = PointStamped()
        position_error_world = PointStamped()

        # state init
        dt = time - self.time
        self.velocity = cur_velocity
        
        x_error = target_p[0] - cur_p[0]
        y_error = target_p[1] - cur_p[1]
        z_error = target_p[2] - cur_p[2]

        # calculate self-velocity in body frame(FLU)
        x_velocity_in_uav, y_velocity_in_uav, z_velocity_in_uav = self.ENU2FLU(cur_velocity[0], cur_velocity[1],
                                                                               cur_velocity[2], yaw)
        # turn to FLU coordinate
        target_p_uav = self.ENU2FLU_in_list(target_p, yaw)
        target_v_uav = self.ENU2FLU_in_list(target_v, yaw)
        target_a_uav = self.ENU2FLU_in_list(target_a, yaw)
        cur_velocity_uav = self.ENU2FLU_in_list(cur_velocity, yaw)
        cur_position_uav = self.ENU2FLU_in_list(cur_p, yaw)

        # calculate position error
        x_error_in_uav = target_p_uav[0] - cur_position_uav[0]
        y_error_in_uav = target_p_uav[1] - cur_position_uav[1]
        z_error_in_uav = target_p_uav[2] - cur_position_uav[2]

        # calculate velocity error
        xv_error_in_uav = \
            self.K_P_pva[0] * x_error_in_uav + \
            self.K_D_pva[0] * (x_error_in_uav - self.position_error_last[0]) / dt + \
            self.K_I_pva[0] + self.i_in_position[0] + \
            target_v_uav[0] - cur_velocity_uav[0]

        yv_error_in_uav = \
            self.K_P_pva[1] * y_error_in_uav + \
            self.K_D_pva[1] * (y_error_in_uav - self.position_error_last[1]) / dt + \
            self.K_I_pva[1] + self.i_in_position[1] + \
            target_v_uav[1] - cur_velocity_uav[1]

        zv_error_in_uav = \
            self.K_P_pva[2] * z_error_in_uav + \
            self.K_D_pv[2] * (z_error_in_uav - self.position_error_last[2]) / dt + \
            self.K_I_pva[2] + self.i_in_position[2] + \
            target_v_uav[2] - cur_velocity_uav[2]

        # calculate control out in body frame
        x_out = \
            self.K_P_pva[3] * xv_error_in_uav + \
            self.K_D_pva[3] * (xv_error_in_uav - self.velocity_error_last[0]) / dt + \
            self.K_I_pva[3] * self.i_in_velocity[0] + \
            target_a_uav[0]

        y_out = \
            self.K_P_pva[4] * yv_error_in_uav + \
            self.K_D_pva[4] * (yv_error_in_uav - self.velocity_error_last[1]) / dt + \
            self.K_I_pva[4] * self.i_in_velocity[1] + \
            target_a_uav[1]

        z_out = \
            self.K_P_pva[5] * zv_error_in_uav + \
            self.K_D_pva[5] * (zv_error_in_uav - self.velocity_error_last[2]) / dt + \
            self.K_I_pva[5] * self.i_in_velocity[2] + \
            target_a_uav[2]

        # filter out and add control deathband
        x_out, y_out, z_out = self.filter_control_out(x_out, y_out, z_out)
        z_out = z_out + self.gravity

        # calculate row pith yaw
        roll = math.asin(
            - y_out / np.sqrt(x_out * x_out + y_out * y_out + z_out * z_out))
        pitch = math.atan(x_out / z_out)
        thrust = np.sqrt(
            x_out * x_out + y_out * y_out + z_out * z_out) / self.gravity * self.hover_throttle  # 0.594 - hover thrust
        # print('roll', roll)
        # print('pitch', pitch)
        # print('thrust', thrust)

        # set the history of control error
        self.position_error_last[0] = x_error_in_uav
        self.position_error_last[1] = y_error_in_uav
        self.position_error_last[2] = z_error_in_uav  # position

        self.velocity_error_last[0] = xv_error_in_uav
        self.velocity_error_last[1] = yv_error_in_uav
        self.velocity_error_last[2] = zv_error_in_uav  # velocity

        # set the history of I error
        if self.i_start_flag:
            self.error_list_position[0].append(x_error_in_uav * dt)
            self.error_list_position[1].append(y_error_in_uav * dt)
            self.error_list_position[2].append(z_error_in_uav * dt)
            self.error_list_velocity[0].append(xv_error_in_uav * dt)
            self.error_list_velocity[1].append(yv_error_in_uav * dt)
            self.error_list_velocity[2].append(zv_error_in_uav * dt)
            self.filter_i_out()

        # set the time
        self.time = time

        # ros publish
        position_error_world.header.stamp = rospy.Time.now()
        position_error_world.point.x = x_error
        position_error_world.point.y = y_error
        position_error_world.point.z = z_error
        position_error_world_pub.publish(position_error_world)
        
        velocity_uav.header.stamp = rospy.Time.now()
        velocity_uav.point.x = x_velocity_in_uav
        velocity_uav.point.y = y_velocity_in_uav
        velocity_uav.point.z = z_velocity_in_uav
        velocity_uav_pub.publish(velocity_uav)

        position_error_uav.header.stamp = rospy.Time.now()
        position_error_uav.point.x = x_error_in_uav
        position_error_uav.point.y = y_error_in_uav
        position_error_uav.point.z = z_error_in_uav
        position_error_pub.publish(position_error_uav)

        velocity_error.header.stamp = rospy.Time.now()
        velocity_error.point.x = xv_error_in_uav
        velocity_error.point.y = yv_error_in_uav
        velocity_error.point.z = zv_error_in_uav
        velocity_error_pub.publish(velocity_error)

        control_uav.header.stamp = rospy.Time.now()
        control_uav.point.x = x_out
        control_uav.point.y = y_out
        control_uav.point.z = z_out
        control_uav_pub.publish(control_uav)
        return roll, pitch, thrust

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<< initial function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    def init_time(self, t):
        self.time = t

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<< filter function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    def filter_control_out(self, x_out, y_out, z_out):
        if abs(x_out) > self.limit[0]:
            x_out = np.sign(x_out) * self.limit[0]
        if abs(y_out) > self.limit[1]:
            y_out = np.sign(y_out) * self.limit[1]
        if abs(z_out) > self.limit[2]:
            z_out = np.sign(z_out) * self.limit[2]

        # control death band
        if abs(x_out) < self.deathband[0]:
            x_out = 0
        if abs(y_out) < self.deathband[1]:
            y_out = 0
        if abs(z_out) < self.deathband[2]:
            z_out = 0

        return x_out, y_out, z_out

    def filter_i_out(self):
        # if error list is too long, give up the error of previous time in I
        for i in range(3):
            if len(self.error_list_position[i]) > self.error_length_I:
                del (self.error_list_position[i][0])
            if len(self.error_list_velocity[i]) > self.error_length_I:
                del (self.error_list_velocity[i][0])

        i_x = sum(self.error_list_position[0])
        i_y = sum(self.error_list_position[1])
        i_z = sum(self.error_list_position[2])
        i_vx = sum(self.error_list_velocity[0])
        i_vy = sum(self.error_list_velocity[1])
        i_vz = sum(self.error_list_velocity[2])
        if abs(i_x) > self.limit_I[0]:
            i_x = np.sign(i_x) * self.limit_I[0]
        if abs(i_y) > self.limit_I[1]:
            i_y = np.sign(i_y) * self.limit_I[1]
        if abs(i_z) > self.limit_I[2]:
            i_z = np.sign(i_z) * self.limit_I[2]
        if abs(i_vx) > self.limit_I[3]:
            i_vx = np.sign(i_vx) * self.limit_I[3]
        if abs(i_vx) > self.limit_I[4]:
            i_vy = np.sign(i_vy) * self.limit_I[4]
        if abs(i_vx) > self.limit_I[5]:
            i_vz = np.sign(i_vz) * self.limit_I[5]  # I limit in position and velocity
        self.i_in_position[0] = i_x
        self.i_in_position[1] = i_y
        self.i_in_position[2] = i_z
        self.i_in_velocity[0] = i_vx
        self.i_in_velocity[1] = i_vy
        self.i_in_velocity[2] = i_vz

    # <<<<<<<<<<<<<<<<<<<<<<<<<<<< coordinate system conversion function >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
    # For Body frame, we will use FLU (Forward, Left and Up)
    #           +Z     +X
    #            ^    ^
    #            |  /
    #            |/
    #  +Y <------body
    # For world frame, we will use ENU (EAST, NORTH and UP)
    #     +Z     +Y
    #      ^    ^
    #      |  /
    #      |/
    #    world------> +X

    #  FLU -> ENU
    def FLU2ENU(self, x, y, z, yaw):
        ENU_x = x * math.cos(yaw) - y * math.sin(yaw)
        ENU_y = x * math.sin(yaw) + y * math.cos(yaw)
        ENU_z = z
        return ENU_x, ENU_y, ENU_z

    def FLU2ENU_in_list(self, p, yaw):
        ENU_x = p[0] * math.cos(yaw) - p[1] * math.sin(yaw)
        ENU_y = p[0] * math.sin(yaw) + p[1] * math.cos(yaw)
        ENU_z = p[2]
        return [ENU_x, ENU_y, ENU_z]

    #  ENU -> FLU   
    def ENU2FLU(self, x, y, z, yaw):
        ENU_x = x * math.cos(yaw) + y * math.sin(yaw)
        ENU_y = - x * math.sin(yaw) + y * math.cos(yaw)
        ENU_z = z
        return ENU_x, ENU_y, ENU_z

    def ENU2FLU_in_list(self, p, yaw):
        FLU_x = p[0] * math.cos(yaw) + p[1] * math.sin(yaw)
        FLU_y = - p[0] * math.sin(yaw) + p[1] * math.cos(yaw)
        FLU_z = p[2]
        return [FLU_x, FLU_y, FLU_z]


